//
//  heartutil.m
//  Light
//
//  Created by WangJiaheng on 16/1/22.
//  Copyright © 2016年 骁唐. All rights reserved.
//

#include "heartutil.h"
#import "AppDelegate.h"
#import "UserConfig.h"


int QRSdetection(double * data_m,int * dataout,int len_m, int length_out)
{
    double *datain=(double *) calloc(len_m,sizeof(double));
    for (int i = 0;i<len_m;i++)
    {//数据
        datain[i] = data_m[i];
    }
    int * QRS_ind;
    QRS_ind = (int * )calloc(100,sizeof(int));
    int QRS_len;
    QRS_len=QRSdetect(datain,len_m,QRS_ind); /// R 波检测
    int * c_array = (int * )calloc(100,sizeof(int));
    
    TJJLog(@"SXW___2'index:%d,lenght:%d",length_out,len_m);
    if (QRS_len<length_out)
    {
        for (int i=0;i<QRS_len;i++)
        {
            c_array[i] = QRS_ind[i];
        }
    }
    else
        QRS_len = 0;
    for (int i = 0; i < length_out; i++)
    {
        dataout[i] = c_array[i];
    }
    free(c_array);
    free(datain);
    free(QRS_ind);
    return QRS_len;
}


double diff_mean(int *datain,int interval1,int interval2){
    int i;
    double temp=0.0;
    
    for(i=interval1;i<interval2;i++){
        temp=temp+datain[i+1]-datain[i];
    }
    return temp/(interval2-interval1);
    
}
double diff_mean2(double *datain,int interval1,int interval2){
    int i;
    double temp=0.0;
    
    for(i=interval1;i<interval2;i++){
        temp=temp+datain[i+1]-datain[i];
    }
    return temp/(interval2-interval1);
    
}
int max(double *data_in,int st,int end){
    int i;
    double temp=data_in[st];
    int index=st;
    for(i=st+1;i<=end;i++){
        if(temp<data_in[i]){
            temp=data_in[i];
            index=i;
        }
    }
    return index;
}

int filter2(double *Bn,double *An,int len1,int len2,
            double *datain,int len3,double *out){
    int i=0;
    int j=0;
    
    int len_m=len1;
    
    if (len1<len2){
        len_m=len2;
    }
    
    double temp1;
    if (len3<len_m){
        printf("the length of data is too short!!");
        return 0;
    }
    
    for(i=0;i<len3;i++){
        
        temp1=datain[i]*Bn[0];
        out[i]=0;
        if (i<len_m){
            for (j=1;j<=i;j++){
                temp1=temp1+datain[i-j]*Bn[j]-out[i-j]*An[j];
            }
        }
        else{
            for(j=1;j<len_m;j++){
                temp1=temp1+datain[i-j]*Bn[j]-out[i-j]*An[j];
            }
        }
        out[i]=temp1;
    }
    return 0;
}

void conv(double *hn,int m,double *epoch,int n,double *dataconv ){
    //	int m=sizeof(hn)/sizeof(double);
    //	int n=sizeof(epoch)/sizeof(double);
    int k;
    int j;
    //double dataconv[]=calloc(m+n-1,sizeof(double));
    for (k=0;k<n;k++){
        int upper=(k<m-1)? k:m-1;
        for (j=0;j<=upper;j++){
            dataconv[k]=dataconv[k]+hn[j]*epoch[k-j];
        }
    }
    //return (dataconv);
}

int moving_average(double *datain,int L,int len,double *out){
    //	int L=sizeof(datain)/sizeof(double);
    int i;
    //    double temp;
    out[0]=datain[0]/len;
    //    int st;
    for(i=1;i<L;i++){
        //		st=(i+1>=len)? i+1-len:0;
        if (i+1<=len){
            //out[i]=(out[i-1]*(i)+datain[i] )/(i+1);
            out[i]=out[i-1]+datain[i] /len;
        }else{
            out[i]=out[i-1]+datain[i]/len-datain[i-len]/len;
        }
        
    }
    return 0;
}

double mean_select(double *data,int n1,int n2){
    int i;
    double sum;
    double aver;
    for(i=n1;i<=n2;i++){
        sum +=*(data+i);
    }
    aver=sum/(n2-n1+1);
    return aver;
}
//////////////////////////////////////////////
//////////////////////////////////////////////
///////////////////////////////////////////////// /

/*
 int QRSdetect(double *datain,int len,int *dataout) {
 int Fs=250;
 int MinDis=round(0.3*Fs);
 int index=0;
 int QRS_len=0;
 int i;
 int k;
 //    int st_flag;
 
 
 int *QRS_ind;
 QRS_ind=dataout;
 ////////////////////////////////////////////
 ////////////////////////////////////////////
 //	double *data_f=(double *) calloc(len,sizeof(double));
 double *data_d=(double *) calloc(len,sizeof(double));
 double *data_m=(double *) calloc(len,sizeof(double));
 //////////////////////////////////////////////////
 //	double An[]={0.0016,0,-0.0047,0,0.0047,0,-0.0016}; //bandpass filter 5-15 Hz
 //	double Bn[]={1.000, -5.3682,12.1371,-14.7948,10.2563,-3.8344,0.6041};
 //
 //	filter2(An,Bn,7,7,datain,len,data_f); // band pass filter 5~15Hz
 /////////////////////////////////
 double h_d[]={-1.0/8,-2.0/8,0,2.0/8,1.0/8};// derivative filter parameters
 ////// derivative filter/////////////
 conv(h_d,5,datain,len,data_d);
 ///////////////squaring /////////////
 for(i=0;i<len;i++){
 data_d[i]=data_d[i]*data_d[i];
 }
 //////moving average ////////////////
 moving_average(data_d,len,round(0.1*Fs),data_m);
 //free(data_f);
 
 ////////////////////////////
 //	FILE *fp1;
 //	char *filename2="moving_data.txt";
 //	fp1=fopen(filename2,"w");
 //	for(i=0;i<len;i++){
 //		fprintf(fp1,"%lf\n",data_m[i]);
 //	}
 //	fclose(fp1);
 
 //    double *QRS_ind=(double *) calloc(100,sizeof(double));
 ////////////////////////////////////////
 //// initialization //////////////////
 ////////////////////////////////////////
 double THR_SIG=0.5*data_m[max(data_m,Fs,3*Fs)];
 double THR_NOISE=0.5*mean_select(data_m,Fs,3*Fs);
 //    double SIG_LEV=THR_SIG;
 //    double NOISE_LEV=THR_NOISE;
 
 
 ////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////
 
 double temp_max=data_m[0];
 double HR_last=0,HR_mean=0;
 int miss_index;
 int QRS_flag=0;
 int mode=0;
 for(i=1;i<len-1;i++)
 {  // starting the for loop
 switch(mode)
 {
 case 0:
 {
 if(data_m[i]>data_m[i-1]&&data_m[i]>=data_m[i+1])
 {
 temp_max=data_m[i];
 index=i;
 k=0;
 mode=1;
 }
 }
 break;
 case 1:
 {
 if(data_m[i]<=temp_max)
 {
 k++;
 if (k>=MinDis)
 {
 QRS_flag=1;
 mode=0;
 k=0;
 }
 }
 else
 {
 i=i-1;
 mode=0;
 }
 }
 break;
 default:
 {
 break;
 }
 }
 
 if(i==len-2 && k>0)
 {
 QRS_flag=1;
 }
 ////////////////////// to check the local maximul whether QRS or not/////////////
 if (QRS_flag==1)
 {
 // update the heart rate
 if (QRS_len>4)
 {
 HR_last=QRS_ind[QRS_len-1]-QRS_ind[QRS_len-2];
 HR_mean=diff_mean(QRS_ind,QRS_len-1-4,QRS_len-1);
 }
 
 // calculate the mean of the last 4 R waves to make sure the QRS is not missing
 if(QRS_len>4)
 {
 //HR_mean=HR_mean;
 }
 else
 {
 HR_mean=Fs;
 }
 
 if (index-round(1.66*HR_mean)>=0&&QRS_len>1){
 if (index-QRS_ind[QRS_len-1]>=round(1.66*HR_mean)){
 miss_index=max(data_m,QRS_ind[QRS_len-1]+round(0.2*Fs),index-round(0.2*Fs));
 if (data_m[miss_index]>THR_NOISE&&data_m[miss_index]<THR_SIG){
 QRS_ind[QRS_len]=miss_index;
 QRS_len++;
 }
 }
 }
 
 // find the noise and peaks
 int skip=0;
 double slope1;
 double slope2;
 
 if (temp_max>THR_SIG&&temp_max<4*THR_SIG){ //&&temp_max<4*THR_SIG
 if (QRS_len>1&&(QRS_ind[QRS_len-1]-round(0.075*Fs))>=0){
 if(index-QRS_ind[QRS_len-1]<=round(0.36*Fs)){
 slope1=diff_mean2(data_m,index-round(0.075*Fs),index);
 slope2=diff_mean2(data_m,QRS_ind[QRS_len-1]-round(0.075*Fs),QRS_ind[QRS_len-1]);
 if(fabs(slope1)<=fabs(0.5*slope2)){
 //		   				NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
 skip=1;
 }
 else{
 skip=0;
 }
 }
 }
 
 if (skip==0){
 QRS_ind[QRS_len]=index;
 QRS_len++;
 // adjust teh signal level
 //		   			SIG_LEV=0.125*temp_max+0.875*SIG_LEV;
 }
 }
 //		   else if(THR_NOISE<=temp_max && temp_max<THR_SIG){
 //		   	NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
 //		   }
 //		   else {
 //		   	NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
 //		   }
 // adjust the threshold with the SNR
 //		    if (NOISE_LEV != 0 || SIG_LEV != 0){
 //		    	THR_SIG = NOISE_LEV + 0.25*(abs(SIG_LEV - NOISE_LEV));
 //            	THR_NOISE = 0.5*(THR_SIG);
 //			}
 QRS_flag=0;
 }
 }
 free(data_d);
 free(data_m);
 //	free(data_f);
 return QRS_len;
 }
 */

//int QRSdetect(double *datain,int len,int *dataout) {
int QRSdetect(double *datain,int len,int *dataout) {
    int Fs=250;
    int MinDis=round(0.3*Fs);
    int index=0;
    int index_orig=0; /// changes adding new variable
    int QRS_len=0;
    int i;
    int k;
    int st_flag;
    
    int QRS_ind[100]; /// changes redefinition
    int *QRS_ind_orig; /// changes adding new arrays
    QRS_ind_orig=dataout;
    
    ////////////////////////////////////////////
    ////////////////////////////////////////////
    //	double *data_f=(double *) calloc(len,sizeof(double));
    double *data_d=(double *) calloc(len,sizeof(double));
    double *data_m=(double *) calloc(len,sizeof(double));
    //////////////////////////////////////////////////
    //	double An[]={0.0016,0,-0.0047,0,0.0047,0,-0.0016}; //bandpass filter 5-15 Hz
    //	double Bn[]={1.000, -5.3682,12.1371,-14.7948,10.2563,-3.8344,0.6041};
    //
    //	filter2(An,Bn,7,7,datain,len,data_f); // band pass filter 5~15Hz
    /////////////////////////////////
    double h_d[]={-1.0/8,-2.0/8,0,2.0/8,1.0/8};// derivative filter parameters
    ////// derivative filter/////////////
    conv(h_d,5,datain,len,data_d);
    ///////////////squaring /////////////
    for(i=0;i<len;i++){
        data_d[i]=data_d[i]*data_d[i];
    }
    //////moving average ////////////////
    moving_average(data_d,len,round(0.1*Fs),data_m);
    //free(data_f);
    
    ////////////////////////////
    //	FILE *fp1;
    //	char *filename2="moving_data.txt";
    //	fp1=fopen(filename2,"w");
    //	for(i=0;i<len;i++){
    //		fprintf(fp1,"%lf\n",data_m[i]);
    //	}
    //	fclose(fp1);
    
    //    double *QRS_ind=(double *) calloc(100,sizeof(double));
    ////////////////////////////////////////
    //// initialization //////////////////
    ////////////////////////////////////////
    double THR_SIG=0.5*data_m[max(data_m,Fs,3*Fs)];
    double THR_NOISE=0.5*mean_select(data_m,Fs,3*Fs);
    double SIG_LEV=THR_SIG;
    double NOISE_LEV=THR_NOISE;
    
    
    ////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////
    
    double temp_max=data_m[0];
    double HR_last=0,HR_mean=0;
    int miss_index;
    int miss_index_orig; /// changes adding new variables
    int QRS_flag=0;
    int mode=0;
    for(i=1;i<len-1;i++){  // starting the for loop
        
        switch(mode){
            case 0:{
                if(data_m[i]>data_m[i-1]&&data_m[i]>=data_m[i+1]){
                    temp_max=data_m[i];
                    index=i;
                    k=0;
                    mode=1;
                }
                break;
            }
                
            case 1:{
                if(data_m[i]<=temp_max){
                    k++;
                    if (k>=MinDis){
                        QRS_flag=1;
                        k=0;
                        mode=0;
                    }
                }
                else {
                    i=i-1;
                    mode=0;
                }
                break;
            }
            default:{
                break;
            }
        }
        
        if(i==len-2 && k>0){
            QRS_flag=1;
        }
        ////////////////////// to check the local maximul whether QRS or not/////////////
        if (QRS_flag==1){
            // update the heart rate
            if (QRS_len>4){
                HR_last=QRS_ind[QRS_len-1]-QRS_ind[QRS_len-2];
                HR_mean=diff_mean(QRS_ind,QRS_len-1-4,QRS_len-1);
            }
            
            // calculate the mean of the last 4 R waves to make sure the QRS is not missing
            if(QRS_len>4){
                HR_mean=HR_mean;
            }
            else{
                HR_mean=Fs;
            }
            
            if (index-round(1.66*HR_mean)>=0&&QRS_len>1){
                if (index-QRS_ind[QRS_len-1]>=round(1.66*HR_mean)){
                    miss_index=max(data_m,QRS_ind[QRS_len-1]+round(0.2*Fs),index-round(0.2*Fs));
                    if (data_m[miss_index]>THR_NOISE&&data_m[miss_index]<THR_SIG){
                        if (miss_index!=QRS_ind[QRS_len-1]){
                            miss_index_orig=max(datain,miss_index-round(0.05*Fs),miss_index); /// changes adding
                            QRS_ind[QRS_len]=miss_index;
                            QRS_ind_orig[QRS_len]=miss_index_orig; /// changes adding
                            QRS_len++;
                            if(miss_index ==2422){
                                printf("miss_index!");
                            }
                        }
                        
                    }
                }
            }
            
            
            // find the noise and peaks
            int skip=0;
            double slope1;
            double slope2;
            
            if (temp_max>THR_SIG&&temp_max<4*THR_SIG){ //&&temp_max<4*THR_SIG
                if (QRS_len>1&&(QRS_ind[QRS_len-1]-round(0.075*Fs))>=0){
                    if(index-QRS_ind[QRS_len-1]<=round(0.36*Fs)){
                        slope1=diff_mean2(data_m,index-round(0.075*Fs),index);
                        slope2=diff_mean2(data_m,QRS_ind[QRS_len-1]-round(0.075*Fs),QRS_ind[QRS_len-1]);
                        if(fabs(slope1)<=fabs(0.5*slope2)){
                            //		   				NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
                            skip=1;
                        }
                        else{
                            skip=0;
                        }
                    }
                }
                
                if (skip==0){
                    //if (index!=QRS_ind[QRS_len-1]){
                    index_orig=max(datain,index-round(0.1*Fs)>0? index-round(0.1*Fs):0,index); /// changes adding
                    QRS_ind[QRS_len]=index;
                    QRS_ind_orig[QRS_len]=index_orig; /// changes adding
                    QRS_len++;
                    if(index==2422){
                        printf("index\n");
                    }
                    
                    //}
                    
                    // adjust teh signal level
                    //		   			SIG_LEV=0.125*temp_max+0.875*SIG_LEV;
                }
            }
            //		   else if(THR_NOISE<=temp_max && temp_max<THR_SIG){
            //		   	NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
            //		   }
            //		   else {
            //		   	NOISE_LEV=0.125*temp_max+0.875*NOISE_LEV;
            //		   }
            // adjust the threshold with the SNR
            //		    if (NOISE_LEV != 0 || SIG_LEV != 0){
            //		    	THR_SIG = NOISE_LEV + 0.25*(abs(SIG_LEV - NOISE_LEV));
            //            	THR_NOISE = 0.5*(THR_SIG);
            //			}
            QRS_flag=0;
        }
        
    }
    
    free(data_d);
    data_d = NULL;
    free(data_m);
    data_m = NULL;
    //	free(data_f);
    return QRS_len;
}


